﻿using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;

namespace Incendios0_1.localization
{
    public enum CoorType { Cartecian, Spherical };

    public struct CylVector3
    {
        public float R;
        public float Th;
        public float Ph;

        public CylVector3(float R, float Th, float Ph)
        {
            this.R = R;
            this.Th = Th;
            this.Ph = Ph;
        }

        public static CylVector3 operator +(CylVector3 a, CylVector3 b)
        {
            return new CylVector3(a.R+b.R,a.Th+b.Th,a.Ph+b.Ph);
        }

        public static CylVector3 operator -(CylVector3 a, CylVector3 b)
        {
            return new CylVector3(a.R - b.R, a.Th - b.Th, a.Ph - b.Ph);
        }

        public static CylVector3 operator *(CylVector3 a, CylVector3 b)
        {
            return new CylVector3(a.R * b.R, a.Th * b.Th, a.Ph * b.Ph);
        }

        public static CylVector3 operator /(CylVector3 a, CylVector3 b)
        {
            return new CylVector3(a.R / b.R, a.Th / b.Th, a.Ph / b.Ph);
        }
    }

    public struct GeoCoord
    {
        public float R; //mtrs
        public float Lat;
        public float Long;

        public GeoCoord(float Lat, float Long)
        {
            this.R = 6371000;
            this.Lat = Lat;
            this.Long = Long;
        }

        public GeoCoord(float Lat, float Long, float Height)
        {
            this.R = 6371000 + Height;
            this.Lat = Lat;
            this.Long = Long;
        }
    }

    public class Vector3
    {
        private AForge.Math.Vector3 v; //Cartecian
        private CylVector3 s; //Spherical

        public float X
        {
            get
            {
                return v.X;
            }

            set
            {
                v.X = value;
                s = Cart2Sphere(v);
            }
        }

        public float Y
        {
            get
            {
                return v.Y;
            }

            set
            {
                v.Y = value;
                s = Cart2Sphere(v);
            }
        }

        public float Z
        {
            get
            {
                return v.Z;
            }

            set
            {
                v.Z = value;
                s = Cart2Sphere(v);
            }
        }

        public float R
        {
            get
            {
                return s.R;
            }

            set
            {
                s.R = value;
                v = Sphere2Cart(s);
            }
        }

        public float Th
        {
            get
            {
                return s.Th;
            }

            set
            {
                s.Th = value;
                v = Sphere2Cart(s);
            }
        }

        public float Ph
        {
            get
            {
                return s.Ph;
            }

            set
            {
                s.Ph = value;
                v = Sphere2Cart(s);
            }
        }

        public Vector3()
        {
            v = new AForge.Math.Vector3(0, 0, 0);
            s = new CylVector3(0, 0, 0);
        }

        public Vector3(float X, float Y, float Z)
        {
            v = new AForge.Math.Vector3(X, Y, Z);
            s = Cart2Sphere(v);
        }

        public Vector3(float X, float Y, float Z, CoorType type)
        {
            if (type == CoorType.Cartecian)
            {
                v = new AForge.Math.Vector3(X, Y, Z);
                s = Cart2Sphere(v);
            }
            else if (type == CoorType.Spherical)
            {
                s = new CylVector3(X, Y, Z);
                v = Sphere2Cart(s);
            }
        }

        public Vector3(AForge.Math.Vector3 data)
        {
            v = data;
            s = Cart2Sphere(v);
        }

        public Vector3(CylVector3 data)
        {
            s = data;
            v = Sphere2Cart(s);
        }

        public Vector3(GeoCoord data)
        {
            s = Geo2Sphere(data);
            v = Sphere2Cart(s);
        }

        public static Vector3 operator +(Vector3 a, Vector3 b)
        {
            return new Vector3(a.v + b.v);
        }

        public static Vector3 operator -(Vector3 a, Vector3 b)
        {
            return new Vector3(a.v - b.v);
        }

        public static Vector3 operator *(Vector3 a, Vector3 b)
        {
            return new Vector3(a.v * b.v);
        }

        public static Vector3 operator *(Vector3 a, float b)
        {
            return new Vector3(a.v * b);
        }

        public static Vector3 operator /(Vector3 a, Vector3 b)
        {
            return new Vector3(a.v / b.v);
        }

        public static Vector3 operator /(Vector3 a, float b)
        {
            return new Vector3(a.v / b);
        }

        public static float dot(Vector3 a, Vector3 b)
        {
            Vector3 vr = a * b;
            return vr.X + vr.Y + vr.Z;
        }

        public GeoCoord getGeo()
        {
            return Sphere2Geo(s);
        }

        public Vector3 getNormal()
        {
            return Normalize(this);
        }

        public static CylVector3 Geo2Sphere(GeoCoord g)
        {
            return new CylVector3(g.R, 90 - g.Lat, g.Long);
        }

        public static GeoCoord Sphere2Geo(CylVector3 g)
        {
            return new GeoCoord(90 - g.Th, g.Ph);
        }

        public static Vector3 Normalize(Vector3 v)
        {
            float nor = Normal(v);
            return (v / nor);
        }

        public static CylVector3 Cart2Sphere(AForge.Math.Vector3 v)
        {
            return Cart2Sphere(v.X, v.Y, v.Z);
        }

        public static AForge.Math.Vector3 Sphere2Cart(CylVector3 s)
        {
            return Sphere2Cart(s.R, s.Th, s.Ph);
        }

        public static CylVector3 Cart2Sphere(float x, float y, float z)
        {
            if (x == 0 && y == 0 && z == 0)
            {
                return new CylVector3(0, 0, 0);
            }
            float r = (float)Math.Sqrt(x * x + y * y + z * z);
            return new CylVector3(r, (float)(Math.Acos(z / r) * 180 / Math.PI), (float)(Math.Atan2(y, x) * 180 / Math.PI));
        }

        public static AForge.Math.Vector3 Sphere2Cart(float r, float th, float ph)
        {
            float cosph = (float)(Math.Cos(ph * Math.PI / 180));
            float sinph = (float)(Math.Sin(ph * Math.PI / 180));
            float costh = (float)(Math.Cos(th * Math.PI / 180));
            float sinth = (float)(Math.Sin(th * Math.PI / 180));

            return new AForge.Math.Vector3(r * sinth * cosph, r * sinth * sinph, r * costh);
        }

        public static float Normal(Vector3 v)
        {
            return dot(v , v);
        }

        internal Vector3 Clone()
        {
            return new Vector3(v);
        }
    }
}
